#include "FollowWayPointsState.h"
#include "PDController.h"
#include "AttractiveField.h"
#include "RandomField.h"
#include "RepulseField.h"

vector<FollowWayPointsState*> FollowWayPointsState::all_states; 

FollowWayPointsState::FollowWayPointsState(vector <Vector2> & waypoints) {

	all_states.push_back(this);

	for (unsigned int i = 0; i < waypoints.size(); i++) {
		_waypoints.push_back(waypoints[i]);
	}

	_done = false;
	cur_waypoint = -1;
	setNextWayPoint();
}

void FollowWayPointsState::setNextWayPoint() {
	int MAX_WAYPOINTS = 10;
	if (cur_waypoint < ((int)_waypoints.size() - 1)) {
		cur_waypoint++;
	}
	else {
		cur_waypoint = -1;
		_done = true;
	}
}


FollowWayPointsState::~FollowWayPointsState(void) {
	for (unsigned int i = 0; i < all_states.size(); i++) {
		if (all_states[i] == this) {
			all_states[i] = 0;
		}
	}
}

vector<Vector2> FollowWayPointsState::all_waypoints() {
	vector<Vector2> waypoints;

	for (unsigned int i = 0; i < all_states.size(); i++) {
		if (all_states[i]) {
			FollowWayPointsState* state = all_states[i];
			for (unsigned int j = state->cur_waypoint; j < state->_waypoints.size(); j++) {
				waypoints.push_back(state->_waypoints[j]);
			}
		}
	}

	return waypoints;
}

Command FollowWayPointsState::GetCommand(Agent* agent) {
	if (cur_waypoint >= 0) {
		tank_t* tank = agent->getTank();
		Vector2 tank_pos(tank->pos[0], tank->pos[1]);
		Vector2 waypoint = _waypoints.at(cur_waypoint);

		AttractiveField field(waypoint);
		RandomField rand_field;
		Vector2 trajectory = field.get_trajectory(tank_pos);
		trajectory += rand_field.get_trajectory(tank_pos);
		
		World* world = World::instance();
		for (unsigned int i = 0; i < world->other_tanks().size(); i++) {
			otank_t* otank = &world->other_tanks().at(i);
			Vector2 otank_pos(otank->pos[0], otank->pos[1]);

			RepulseField rfield(otank_pos);
			trajectory += rfield.get_trajectory(tank_pos);
		}
	
		double dist = (waypoint - tank_pos).magnitude();
		
		bool lastwaypoint = (_waypoints[_waypoints.size() - 1] - tank_pos).magnitude() < 50;
		double threshold = lastwaypoint ? 5 : 50;

		if (dist < threshold) {
			setNextWayPoint();
		}

		Command cmd(-1, 0, 0, false);

		World::instance()->flagBearerDir(trajectory);

		cmd = PDController::instance()->GetCommand(tank, trajectory);
		return cmd;
	}
	else {
		return Command(agent->getTank()->index, 0, 0, false, false);
	}
}

bool FollowWayPointsState::done() {
	return _done;
}
